#pragma once

#include <Eigen/Eigen>
#include <bits/stdc++.h>
#include <ros/ros.h>
#include <trajectory_msgs/JointTrajectoryPoint.h>

void setPVA(const ros::Publisher &pva_pub, Eigen::Vector3d p, Eigen::Vector3d v,
            Eigen::Vector3d a, double yaw = 0.0) {
  trajectory_msgs::JointTrajectoryPoint pva_setpoint;

  pva_setpoint.positions.push_back(p(0)); // x
  pva_setpoint.positions.push_back(p(1)); // y
  pva_setpoint.positions.push_back(p(2)); // z
  pva_setpoint.positions.push_back(yaw);

  pva_setpoint.velocities.push_back(v(0));
  pva_setpoint.velocities.push_back(v(1));
  pva_setpoint.velocities.push_back(v(2));

  pva_setpoint.accelerations.push_back(a(0));
  pva_setpoint.accelerations.push_back(a(1));
  pva_setpoint.accelerations.push_back(a(2));

  pva_pub.publish(pva_setpoint);
}

double CalYaw(double x, double y, double z, double w) {
  return atan2(2.0 * (w * z + x * y), 1.0 - 2.0 * (y * y + z * z));
}

std::vector<Eigen::Vector3d> naive_traj_plan(const Eigen::Vector3d &start_p,
                                             const Eigen::Vector3d &end_p,
                                             double rate, double v = 1.0) {
  double delt_t = 1.0 / rate;
  std::vector<Eigen::Vector3d> traj;
  double total_t = (end_p - start_p).norm() / v;
  for (int i = 0; i * delt_t <= total_t; i++) {
    Eigen::Vector3d pos = start_p + i * delt_t / total_t * (end_p - start_p);
    traj.push_back(pos);
  }
  return traj;
}

std::vector<Eigen::Vector3d> triangle_traj_plan(const Eigen::Vector3d &start_p,
                                                const Eigen::Vector3d &end_p,
                                                double rate,
                                                double max_v = 1.0) {
  double delt_t = 1.0 / rate;
  std::vector<Eigen::Vector3d> traj;
  double total_t = (end_p - start_p).norm() * 2.0 / max_v;
  Eigen::Vector3d v_norm = (end_p - start_p).normalized();
  Eigen::Vector3d a = v_norm * max_v / (0.5 * total_t);
  for (int i = 0; i * delt_t < 0.5 * total_t; i++) {
    double current_t = i * delt_t;
    Eigen::Vector3d point = start_p + 0.5 * a * current_t * current_t;
    traj.push_back(point);
  }
  a=-a;
  Eigen::Vector3d acc_p=traj.back();
  for (int i = 0; i * delt_t < 0.5 * total_t; i++) {
    double current_t = i * delt_t;
    Eigen::Vector3d point = acc_p +v_norm * max_v*current_t+ 0.5 * a * current_t * current_t;
    traj.push_back(point);
  }
  return traj;
}